#include "Bar.h"

Bar::Bar(void)
{
	length = 10.0;
	width = 1.0;
	color = vector3(1.0, 1.0, 1.0);
}

Bar::Bar(double l, double w, double m)
{
	length = l;
	width = w;
	color = vector3(1.0, 1.0, 1.0);
	transform.mass = m;
	transform.invMass = 1/m;
	transform.inertia = (m*l*l)/12;
	transform.invInertia = 1/transform.inertia;
}

Bar::~Bar(void)
{
}

void Bar::AddTorque(vector3 force, double length)
{
	// Value placeholders
	vector3 r;
	vector3 t;
	double mag;

	// Add torque from a given force and the distance from the hinge
	r = quaternion::rotate(vector3::Right(), transform.orientation)  * length;
	t = r.cross(force).normalized();
	mag = length * force.length() * sin(vector3::angle(r, force));
	transform.torque += t * mag;
}

vector3 Bar::GetMomentum(vector3 p)
{
	// Value placeholders
	vector3 r;
	vector3 m;
	double l;
	double mag;

	// Get the displacement vector along the bar
	vector3 toCenter = p - transform.position;
	r = quaternion::rotate(vector3::Right(), transform.orientation);
	l = toCenter.dot(r);

	// Calculate linear momentum vector perpendicular to point
	m = transform.angularMomentum.getVector().cross(r);
	mag = transform.angularMomentum.length()/(length - l);
	return m.normalized() * mag;
}

void Bar::Render(void)
{
	// Calculate axis and angle of rotation
	double angle = 2 * acos(transform.orientation.w) * 180/pi;
	vector3 axis = vector3(transform.orientation.x, 
						   transform.orientation.y, 
						   transform.orientation.z);
	axis.normalize();

	glLoadIdentity();
	glTranslated(transform.position.x, transform.position.y, transform.position.z);	
	glRotated(angle, axis.x, axis.y, axis.z);

	double hw = width/2;
	double hl = length/2;

	vector3 tl = vector3(-hl, hw, 0);
	vector3 tr = vector3(hl, hw, 0);
	vector3 br = vector3(hl, -hw, 0);
	vector3 bl = vector3(-hl, -hw, 0);

	DrawTools::drawQuad(tl, tr, br, bl, color);
}